/*
 * @file px4_simple_flight.cpp
 * @brief PX4无人机起飞-悬停-降落控制(C++/MAVROS)
 * 功能：自主解锁、切Offboard模式、起飞至指定高度、悬停固定时间、自动降落
 */
#include "config.h"


int main(int argc, char **argv) {
    if(argc == 2){
        hover_alt = atof(argv[1]);
        printf("Change the hover_alt to:%f\n", hover_alt);
    }
    // ROS节点初始化
    ros::init(argc, argv, "px4_flight_control");
    ros::NodeHandle nh;
    
    while(!ros::ok()){

    }

    // 创建ROS对象
    ros::Publisher show_status_pub = nh.advertise<get_states::show_status>(
        "show_status", 10);
    // ros::Subscriber flight_sub = nh.subscribe<get_states::flight_status>(
    //     "flight_status", 10, flight_cb);
    ros::ServiceServer flight_statu_srv = nh.advertiseService(
        "flight_status", flight_cb);
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>(
        "mavros/state", 10, state_cb);
    ros::Subscriber local_pos_sub = nh.subscribe<nav_msgs::Odometry>(
        "/mavros/global_position/local", 10, get_local_pose);
    ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>(
        "/mavros/setpoint_raw/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>(
        "mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>(
        "mavros/set_mode");
    ros::ServiceClient set_land_client = nh.serviceClient<mavros_msgs::CommandTOL>(
        "/mavros/cmd/land");

    //初始化目标位置
    mavros_msgs::PositionTarget target_pose;

    // 设置target_pose坐标系（LOCAL_NED 或 BODY_NED）
    target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;

    // 仅启用速度控制
    target_pose.type_mask = ControlMasks::VELOCITY_CONTROL;
        
    target_pose.header.stamp = ros::Time::now();
    // target_pose.header.frame_id = "odom";  // 或"map"

    target_pose.velocity.x = 0;
    target_pose.velocity.y = 0;
    target_pose.velocity.z = 0;
    // target_pose.yaw = 0.0;
    // target_pose.yaw_rate = 0.0;

    // 控制频率(必须>2Hz)
    ros::Rate rate(50.0);

    // 等待MAVROS与飞控连接
    while(ros::ok() && !current_state.connected) {
        ros::spinOnce();
        rate.sleep();
    }
    ROS_INFO("FCU connected");

    // 发送预设点激活Offboard
    for(int i=100; ros::ok() && current_state.connected && i>0; --i){
        local_pos_pub.publish(target_pose);
        ros::spinOnce();
        rate.sleep();
    }

    // 设置Offboard模式
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
    
    // 解锁
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    // 着陆
    mavros_msgs::CommandTOL land_cmd;
    land_cmd.request.altitude = 0;

    // 状态变量初始化
    ros::Time last_request = ros::Time::now();
    ros::Time hover_start_time;

    // 飞行状态播报
    get_states::show_status show_status;

    double theta = 0;
    double omega = 2 * M_PI / 5;  // 角速度（弧度/秒）
    double r = 3;    //半径
    
    // 主控制循环
    while(ros::ok() && current_state.connected) {
        // 持续发布无人机状态
        show_status.statu = enum_to_str(phase);
        show_status_pub.publish(show_status);
        // 持续发布目标位置
        local_pos_pub.publish(target_pose);
        float dist = abs(current_local_pose.pose.position.z - hover_alt);
        // ROS_INFO("Flying");
        // 状态机逻辑
        switch(phase) {
        case WAITING:
            break;
        
        case INIT:
            if(ros::Time::now() - last_request > ros::Duration(5.0)) {
                if(set_mode_client.call(offb_set_mode) && 
                   offb_set_mode.response.mode_sent) {
                    ROS_INFO("Offboard enabled");
                    phase = ARMING;
                }
                last_request = ros::Time::now();
            }
            break;
            
        case ARMING:
            if(!current_state.armed && 
               (ros::Time::now() - last_request > ros::Duration(5.0))) {
                if(arming_client.call(arm_cmd) && 
                   arm_cmd.response.success) {
                    ROS_INFO("Vehicle armed");
                    // phase = TAKEOFF;
                    phase = ARM_FIN;
                    last_request = ros::Time::now();
                }
            }
            break;
            
        case TAKEOFF:
            // 高度到达阈值判断
            if(current_state.armed && 
               current_state.mode == "OFFBOARD") {
                if(ros::Time::now() - last_request > ros::Duration(takeoff_timeout)) {
                    ROS_ERROR("Takeoff timeout!");
                    phase = LAND;
                } 
                else if(hover_alt - current_local_pose.pose.position.z > 0.1) {  // 此处替换为地面接触检查
                    target_pose.velocity.z = (hover_alt - current_local_pose.pose.position.z) / 2;
                }
                // 实际项目中应添加位置反馈检查
                else if(current_local_pose.pose.position.z + 0.1 >= hover_alt) {  // 此处替换为高度检查逻辑
                    // 切换为本地坐标系
                    target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
                    target_pose.velocity.z = 0;
                    hover_start_time = ros::Time::now();
                    phase = HOVER;
                    ROS_INFO("Reached target height, Hovering...");
                }
            }
            break;
            
        case HOVER:
            // if(flight_statu.statu) {
                // ROS_INFO("Hover complete. Initiating landing...");
                // 设置降落目标高度为地面
                // target_pose.position.z = 0;
                // 切换为机体坐标系
                target_pose.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
                target_pose.velocity.x = 0;
                target_pose.velocity.y = 0;
                // target_pose.velocity.z = -current_local_pose.pose.position.z / 4;
                target_pose.velocity.z = 0;
                // target_pose.yaw = 0.0;
                // phase = LAND;
            // }
            // else{
            //     // 计算当前时间对应的角度
            //     double elapsed = (ros::Time::now() - hover_start_time).toSec();
            //     theta = omega * elapsed;
                
            //     // 计算局部坐标系中的位置（相对于圆心）
            //     double x_local = r * cos(theta) + 0;
            //     double y_local = r * sin(theta) + 1;
                
            //     // 计算切向速度（垂直于半径方向）
            //     double vx = -r * omega * sin(theta);
            //     double vy = r * omega * cos(theta);
                
            //     // 设置目标消息
            //     target_pose.velocity.x = vx;
            //     target_pose.velocity.y = vy;
                
            //     // 计算朝向圆心所需的偏航角
            //     double yaw = atan2(-y_local, -x_local);
            //     target_pose.yaw = yaw;
            // }
            break;
            
        case LAND:
            // 高度<0.1m认为降落完成
            // 实际项目应添加位置反馈检查
            // else if(current_local_pose.pose.position.z < 0.3 && current_local_pose.pose.position.z >= 0.1) {  // 此处替换为地面接触检查
            //     // target_pose.type_mask = ControlMasks::POSITION_CONTROL;
            //     // target_pose.position.x = 0;
            //     // target_pose.position.y = 0;
            //     // target_pose.position.z = 0;
            //     target_pose.velocity.z = -0.05;
            // }
            if(current_local_pose.pose.position.z > 0.05) {  // 此处替换为地面接触检查
                target_pose.velocity.z = -current_local_pose.pose.position.z / 4;
            }
            else if(current_local_pose.pose.position.z <= 0.05) {  // 此处替换为地面接触检查
                // 发送降落指令
                set_land_client.call(land_cmd);
                if(land_cmd.response.success) {
                    ROS_INFO("Landing enabled");
                    phase = DISARM;
                    ros::Duration(5.0).sleep();
                }
            }
            break;
            
        case DISARM:
            arm_cmd.request.value = false;  // 发送上锁命令
            if(arming_client.call(arm_cmd) && arm_cmd.response.success) {
                ROS_INFO("Vehicle disarmed");
                // return 0;  // 程序正常结束
                phase = WAITING;
            }
            break;
        }
        
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}